#include <pid.h>

struct {
	 
	float shijizhi;
	float qingwanzhi;
	float error;
	float error_next;
	float error_last;
	float kp,ki,kd;
	float voltage;
	float integral;
	float max,min;

}pid;


void pid_init()
{
	pid.kp=0.5;
	pid.ki=0.015;
	pid.kd=0.20;
	pid.error=0.0;
	pid.error_next=0.0;
	pid.error_last=0.0;
	pid.shijizhi=0.0;
	pid.qingwanzhi=0.0;
	pid.voltage=0.0;
	pid.integral=0.0;
	
}


float main_pid(float speed)
{
	float index;
	pid.min=speed*(-1);
	pid.max=speed*(2);
	pid.qingwanzhi=speed;
	pid.error=pid.qingwanzhi-pid.shijizhi;
	if(pid.shijizhi>pid.max)//积分限幅
	{
		if(pid.error>speed)//积分分离
		{
			index=0;
		}else{
						index=1;
						if(pid.error<0)
						{
							 pid.integral+=pid.error;
						}
				 }
	}else if(pid.shijizhi<pid.min)
	{
		if(pid.error>speed)
		{
			index=0;
		}else {
						index=1;
						if(pid.error>0)
						{
							pid.integral+=pid.error;
						}
					}
	}else
	{
		if(pid.error>speed)
		{
			index=0;
		}else if(pid.error<(speed-speed*0.1))
		{
				index=1;
				pid.integral+=pid.error;//变积分
		}else
		{
			index=(speed-pid.error)/20;//变积分
			pid.integral+=pid.error;
		}
	}
	
	pid.voltage=pid.kp*pid.error+index*pid.ki*pid.integral+pid.kd*(pid.error-pid.error_next);
	pid.error_next=pid.error;
	pid.shijizhi=pid.voltage*1.0;
	return pid.shijizhi;
}


